%Author:Xiyin,li
%Date  :2021-07-06
%Copyright 2021 Xiyin,li.

%% Introfuction
%Calculate the torqe between permanent magnet
%By Dipole Model and Magnet Moment
%       |z
%       |
%       |_________y
%      /
%   x/
clc,clear
close all
%% STEP1:Set Parameters
Br = 1.465;
nu0 = 4*pi*10^(-7);
effector.radius = 5e-3;
effector.diameter = 2*effector.radius;
effector.height = 10e-3;
effector.volume = pi*(effector.radius^2)*effector.height;
effector.magDirection = [0;-1;0];
effector.postion = [0;0;0];
effector.attitude0 = [0;0;0];

driver.radius = 15e-3;
driver.diameter = 2*driver.radius;
driver.height = 30e-3;
driver.volume = pi*(driver.radius^2)*driver.height;
driver.magDirection = [0;0;1];
driver.position = [0;100e-3;0];
driver.attitude0 = [pi/2;0;0];

%% STEP2:Calculate Magnetic Moment[01].
effector.magMoment = effector.magDirection*(Br*effector.volume)/nu0;
  driver.magMoment =   driver.magDirection*(Br*  driver.volume)/nu0;

%% STEP3:Calculate Magnetic induction
effector.magInduction.Ana = PMCir3DByRadius(Br,effector.diameter,effector.height,driver.position);
effector.magInduction.Dip =           PMDipCir(effector.radius,  effector.height,driver.position,'y');


%% STEP4:Calculate Torqe[02]
Torqe.effector2driver = [];
Torqe.driver2effector = [];
Torqe.driver2effector1 = [];
for theta = linspace(0,pi*2,361)
    driver.attitude = [theta;0;0]+driver.attitude0;
    driver.rotationMatrix = Rot3(driver.attitude);
    effector.attitude = [0;0;0]+effector.attitude0;
    effector.rotationMatrix = Rot3(effector.attitude);
    driver.magInduction.Ana = PMCir3D(Br,driver.diameter,driver.height,-driver.rotationMatrix*driver.position);
    driver.magInduction.Dip = PMDipCir(driver.radius,driver.height,-driver.rotationMatrix*driver.position,'z');
    Torqe.effector2driver = [Torqe.effector2driver cross(driver.rotationMatrix'*driver.magMoment,effector.magInduction.Dip)];
    Torqe.driver2effector = [Torqe.driver2effector cross(effector.magMoment,driver.rotationMatrix'*driver.magInduction.Dip)];
     Torqe.driver2effector1 = [Torqe.driver2effector1 cross(effector.magMoment,driver.rotationMatrix'*driver.magInduction.Ana)];
end

%% STEP5:Debug & Plot
figure(1)
hold on;grid on
set(gca,'FontSize',60,'Fontname', 'Times New Roman');
%plot(linspace(0,360,360),Torqe.effector2driver(1,:),'b-','lineWidth',2);
% plot(linspace(0,360,361),-Torqe.driver2effector1(1,:),'b-o','lineWidth',6);
% plot(linspace(0,360,361),-Torqe.driver2effector(1,:),'r-*','lineWidth',6);

% xlabel('\theta (deg)');
% ylabel('\tau (N\cdot m)')
% legend('Analytical Model','Dipole Model')
axis([0,360,-2.5e-3,2.5e-3])
%plot(linspace(0,360,361),-0.0022636*sin(linspace(0,pi*2,361)),'b-o','lineWidth',2)
plotGif([0:360]/1000,-Torqe.driver2effector1(1,:),-Torqe.driver2effector(1,:),'TorqeByAnaDip')
%% Summary
%% Reference
%[01] https://en.wikipedia.org/wiki/Magnetic_moment
%[02] 

%% Libary
function R = Rot3(attitude)
ax = attitude(1);
by = attitude(2);
cz = attitude(3);
Rz = [cos(cz) -sin(cz) 0;sin(cz) cos(cz) 0;0 0 1];
Ry = [cos(by) 0 sin(by);0 1 0;-sin(by) 0 cos(by)];
Rx = [1 0 0;0 cos(ax) -sin(ax);0 sin(ax) cos(ax)];
R = Rz*Ry*Rx;
end

